#include "slam.h"
#include "mapviewer.h"
#include "stuff/timers.h"
#include <aruco/posetracker.h>
#include <Eigen/Geometry>
#include <opencv2/imgproc/imgproc_c.h>
#include "que.h"
#include <thread>
#include <opencv2/viz/vizcore.hpp>

cv::viz::Viz3d viz("aa");

class frame
{
public:
    cv::Mat depth, disp, color;
    cv::Mat cv_cloud, cv_color;
    cv::Mat cv_pose;
    float dis = 0.001;
    cv::Mat cam_K;
    frame()
    {
        cv::FileStorage fs("/home/u20/ros/src/MarkerSLAM/camera1.yml", cv::FileStorage::READ);
        fs["camera_matrix"] >> cam_K;
    }
    void depth2cloud()
    {
        assert(!depth.empty());
        // cv_cloud = cv::Mat();
        // cv_color = cv::Mat();
        float fx = cam_K.at<float>(0, 0), fy = cam_K.at<float>(1, 1), cx = cam_K.at<float>(0, 2), cy = cam_K.at<float>(1, 2);
        for (int row = 0; row < depth.rows; row++)
            for (int col = 0; col < depth.cols; col++)
            {
                uint16_t dzi = depth.at<uint16_t>(row, col);
                if (!dzi)
                    continue;
                // auto _p = color.at<cv::Vec3b>(row, col);
                float dz = dzi * dis;
                if (dz > 5.0f)
                    continue;

                cv::Vec3f vec;
                vec[0] = dz * (col - cx) / fx;
                vec[1] = dz * (row - cy) / fy;
                // vec[0] = dz * col / fx; // dz * (col - cx) / fx;
                // vec[1] = dz * row / fy; // * (row - cy) / fy;
                vec[2] = dz * 1;

                cv_cloud.push_back(vec);
                cv_color.push_back(color.at<cv::Vec3b>(row, col));
                // }
            }
        // std::cout << cam_K << endl;
    }

    int mcnt = 0;
};
void readFromFile()
{

    cv::FileStorage fs("/home/u20/ros/src/MarkerSLAM/build/markermap.yml", cv::FileStorage::READ);
    int aux = 0;
    // look for the nmarkers
    cv::FileNode markers = fs["aruco_bc_markers"];
    int i = 0;
    cv::Mat Pose;
    cv::Mat points;

    fs["pose"] >> Pose;
    for (cv::FileNodeIterator it = markers.begin(); it != markers.end(); ++it, i++)
    {
        cv::FileNode FnCorners = (*it)["corners"];
        // FnCorners2 >> Pose;
        std::cout << cv::Affine3f(Pose).inv().matrix << std::endl;

        for (cv::FileNodeIterator itc = FnCorners.begin(); itc != FnCorners.end(); ++itc)
        {
            vector<float> coordinates3d;
            // std::cout << (itc) << std::endl;

            (*itc) >> coordinates3d;
            cv::Point3f point(coordinates3d[0], coordinates3d[1], coordinates3d[2]);
            std::cout << point << std::endl;
            points.push_back(point);
            aux++;
            if (aux == 4)
            {
                viz.showWidget("warp_fielsssssssd", cv::viz::WCloud(points), cv::Affine3f(Pose));
                return;
            }
        }
    }
    std::cout << "point" << std::endl;
}

void savePosesToFile(string filename, const std::map<int, cv::Mat> &fmp);
class CmdLineParser
{
    int argc;
    char **argv;

public:
    CmdLineParser(int _argc, char **_argv) : argc(_argc), argv(_argv) {}
    bool operator[](string param)
    {
        int idx = -1;
        for (int i = 0; i < argc && idx == -1; i++)
            if (string(argv[i]) == param)
                idx = i;
        return (idx != -1);
    }
    string operator()(string param, string defvalue = "-1")
    {
        int idx = -1;
        for (int i = 0; i < argc && idx == -1; i++)
            if (string(argv[i]) == param)
                idx = i;
        if (idx == -1)
            return defvalue;
        else
            return (argv[idx + 1]);
    }
};
#include <ros/ros.h>
#include "std_msgs/Float32MultiArray.h"
#include "std_msgs/String.h"
int main222(ImageSyncQueue<Frame *> *pfpose)
{
    std::fstream fss("/home/u20/ros/src/markermapper/img/2023_05_09_16_05_02/file.txt", std::ios::in);
    // cv::FileStorage fs("/home/u20/ros/src/markermapper/img/2023_05_09_16_05_02/pose.yml", cv::FileStorage::READ);
    // cv::Mat idx;
    // fs["idx"] >> idx;
    // std::cout << idx << std::endl;

    std::vector<string> as;
    for (int i = 0; fss.good(); ++i)
    {
        string s1;
        fss >> s1;
        as.push_back("/home/u20/ros/src/markermapper/img/2023_05_09_16_05_02" + s1);
    }
    as.pop_back();
    fss.close();

    ucoslam::Slam Slam;
    ucoslam::ImageParams image_params;
    ucoslam::Params params;
    cv::Mat in_image, depth;
    image_params.readFromXMLFile("/home/u20/桌面/rec/ui/MarkerSLAM/camera.yml");
    params.readFromYMLFile("/home/u20/ros/src/MarkerSLAM/arucoConfig1.yml");

    auto TheMap = std::make_shared<ucoslam::Map>();
#if 1
    // Create the viewer to see the images and the 3D
    auto TViewer = ucoslam::MapViewer::create("Cv");
    TViewer->set("showNumbers", "1");
    TViewer->set("canLeave", "1");
    TViewer->set("mode", "0");
    TViewer->set("modelMatrix", "0.998437 -0.0490304 0.0268194 0  0.00535287 0.561584 0.827403 0  -0.0556289 -0.825967 0.560969 0  0 0 0 1");
    TViewer->set("viewMatrix", " 1 0 0 0.01  0 4.63287e-05 -1 0.910185  0 1 4.63287e-05 9.18  0 0 0 1 ");
#endif
    Slam.setParams(TheMap, params);
    cout << "===@params=== \n";
    cout << "Marker size " << params.aruco_markerSize << endl;
    cout << "min distance " << params.minBaseLine << endl;
    cout << "min err Ratio " << params.aruco_minerrratio_valid << endl;
    cout << "fx=" << image_params.fx() << ", fy=" << image_params.fy() << endl;
    cout << "cx=" << image_params.cx() << ", cy=" << image_params.cy() << endl;
    cout << "distorsion = \n"
         << image_params.Distorsion << endl;
    //        //Ok, lets start
    ucoslam::TimerAvrg Fps;
    char k = 0;
    std::map<int, cv::Mat> frame_pose_map1; // set of poses and the frames they were detected

    cv::viz::Viz3d viz("bb");
    viz.showWidget("coor", cv::viz::WCoordinateSystem(1.0));
    frame f2;
    vector<cv::Affine3f> paths;
    int avc=1;
    char *asvg[]={"aas"};
    ros::init(avc,asvg, "talker"); // 节点名称
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<std_msgs::Float32MultiArray>("robot_pose", 3);
    for (int j = 0; j < as.size() / 2; j++)
    {
        int currentFrameIndex = j;
        in_image = cv::imread(as[j * 2 + 1]);
        f2.color = in_image.clone();
        f2.depth = cv::imread(as[j * 2], 2);

        cv::Mat pose = Slam.process_input(in_image, image_params, currentFrameIndex, f2.depth);
        if (!pose.empty())
        {
            paths.push_back(cv::Affine3f(pose));
            frame_pose_map1.insert({currentFrameIndex, pose});
        }

        Frame *f = new Frame();

        // std_msgs::Float32MultiArray pose_msg;
        // pose_msg.data.resize(17*20); // 这个是设置data大小的
        int jk = 0;
        for (auto &it : Slam.TheMap->keyframes)
        {
            cv::Mat p = it.pose_f2g.inv();
            // f->pose.push_back(p);
            // pose_msg.data[jk * 17] = it.fseq_idx;
            // memcpy(&pose_msg.data[jk * 17 + 1],p.ptr<float>(),16*sizeof(float));
            // jk++;
            // f->idx.push_back(cv::Vec3i(it.fseq_idx));
            // cout << it.fseq_idx << ":" << p << endl;
        }

        // pub.publish(pose_msg);

        // if (j > 500)
        //     pfpose->Push(f);

        continue;
        // cout<<"Image "<<currentFrameIndex<<" fps="<<1./Fps.getAvrg()<<endl;
        //  cout << pose << endl;
        //  Slam.printKeyPose();
        //  Frame *f = new Frame();
        //  f2.cv_cloud = Mat();
        //  for (auto &it : Slam.TheMap->keyframes)
        //  {
        //      cv::Mat p = it.pose_f2g;
        //      f->pose.push_back(p);
        //      f->idx.push_back(cv::Vec3i(it.fseq_idx));

        //     in_image = cv::imread(as[it.fseq_idx * 2 + 1]);
        //     f2.color = in_image.clone();
        //     f2.depth = cv::imread(as[it.fseq_idx * 2], 2);
        if (j % 30 == 0)
        {
            f2.depth2cloud();
            viz.showWidget("warp_field" + to_string(currentFrameIndex), cv::viz::WCloud(f2.cv_cloud, f2.cv_color), cv::Affine3f(Slam.getCurrentPose_f2g()));
        }

        //     // cout << it.fseq_idx << ":" << p << endl;
        // }

        if (paths.size() > 0)
        {
            // paths= paths.reshape(0, 16); //修改行数和通道数
            // std::cout << "通道: " << paths.channels() << std::endl;
            viz.showWidget("path", cv::viz::WTrajectory(paths, cv::viz::WTrajectory::FRAMES));
        }
        // viz.showWidget("coor" + to_string(currentFrameIndex), cv::viz::WCoordinateSystem(1.0), cv::Affine3f(Slam.getCurrentPose_f2g()));
        // ,
        viz.spinOnce(3, false);

        // TViewer->m[0]=, TViewer->m[1]=f2.cv_color;
        // k = TViewer->show(TheMap, in_image, Slam.getCurrentPose_f2g(), "#" + std::to_string(currentFrameIndex) + " fps=" + to_string(1. / Fps.getAvrg()),f2.cv_cloud,f2.cv_color);
    }

    cv::Mat p1, pi;
    int j = 0;
    for (auto &it : Slam.TheMap->keyframes)
    {
        cv::Mat p = it.pose_f2g.inv();
        p1.push_back(p);
        pi.push_back(cv::Vec3i(it.fseq_idx));
        // cout << it.fseq_idx << ":" << p << endl;
    }
    cv::FileStorage fs22("mpose4.yml", cv::FileStorage::WRITE);
    fs22 << "pose" << p1;
    fs22 << "idx" << pi;
    // fs22 << "ps" << ps;

    Frame *f = new Frame();
    for (auto &it : Slam.TheMap->keyframes)
    {
        cv::Mat p = it.pose_f2g.inv();
        f->pose.push_back(p);
        f->idx.push_back(cv::Vec3i(it.fseq_idx));
        // cout << it.fseq_idx << ":" << p << endl;
    }

    cv::FileStorage fs("mpose.yml", cv::FileStorage::WRITE);
    fs << "pose" << f->pose;
    fs << "idx" << f->idx;
    fs.release();
    //        savePosesToFile("pose.txt",frame_pose_map1);
    //        cerr<<"The poses have been saved to "<<argv[6]<<endl;
    // cout << "Image = " << Slam.num_of_image << ", tracking = " << Slam.num_of_success;
    // double rate = Slam.num_of_success / float(Slam.num_of_image);
    // cout << ", Tracking rate = " << rate << endl;
    // cerr << "The markermap is saved to markermap.yml" << endl;
    // TheMap->saveToMarkerMap("markermap.yml");
}

void getQuaternionAndTranslationfromMatrix44(const cv::Mat &M_in, double &qx, double &qy, double &qz, double &qw, double &tx, double &ty, double &tz)
{
    // get the 3d part of matrix and get quaternion
    assert(M_in.total() == 16);
    cv::Mat M;
    M_in.convertTo(M, CV_64F);
    cv::Mat r33 = cv::Mat(M, cv::Rect(0, 0, 3, 3));
    // use now eigen
    Eigen::Matrix3f e_r33;
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++)
            e_r33(i, j) = M.at<double>(i, j);

    // now, move to a angle axis
    Eigen::Quaternionf q(e_r33);
    qx = q.x();
    qy = q.y();
    qz = q.z();
    qw = q.w();

    tx = M.at<double>(0, 3);
    ty = M.at<double>(1, 3);
    tz = M.at<double>(2, 3);
}

void savePosesToFile(string filename, const std::map<int, cv::Mat> &fmp)
{
    std::ofstream file(filename);
    double qx, qy, qz, qw, tx, ty, tz;
    for (auto frame : fmp)
    {
        if (!frame.second.empty())
        {
            cv::Mat minv = frame.second.inv();
            getQuaternionAndTranslationfromMatrix44(minv, qx, qy, qz, qw, tx, ty, tz);
            file << frame.first << " " << tx << " " << ty << " " << tz << " " << qx << " " << qy << " " << qz << " "
                 << qw << endl;
        }
    }
}

void funcmask(ImageSyncQueue<Frame *> *pqmask)
{

    // std::string fileName = "/home/u20/bishe/src/ros_elas/config.yaml"; // YAML
    // cv::FileStorage fs2(fileName, cv::FileStorage::READ);
    // float dis;
    // cv::Mat cam_k;
    // fs2["dis"] >> dis;
    // fs2["cam_K_vec"] >> cam_k;
    // std::cout << cam_k << endl;
    viz.showWidget("cooss1r", cv::viz::WCoordinateSystem(1.0));

    std::fstream fss("/home/u20/ros/src/markermapper/img/2023_05_09_16_05_02/file.txt", std::ios::in);
    // cv::FileStorage fs("/home/u20/ros/src/markermapper/img/2023_05_09_16_05_02/pose.yml", cv::FileStorage::READ);

    // cv::Mat idx;
    // fs["idx"] >> idx;
    // std::cout << idx << std::endl;

    std::vector<string> as;
    for (int i = 0; i < 2000; ++i)
    {
        string s1;
        fss >> s1;
        as.push_back("/home/u20/ros/src/markermapper/img/2023_05_09_16_05_02" + s1);
    }
    fss.close();
    frame f;
    Frame *pf = new Frame;

    cv::FileStorage fs("mpose2.yml", cv::FileStorage::READ);
    fs["pose"] >> pf->pose;
    fs["idx"] >> pf->idx;
    cv::Mat ps;
    fs["ps"] >> ps;
    fs.release();
    //
    // cv::Mat c(4, 4, CV_32FC1, pfsss + r * 16);
    cout << pf->idx << endl;
    vector<cv::Affine3f> paths;
    paths.push_back(cv::Affine3f::Identity());
    cout << ps.rows / 4 << endl;
    float *pfsss = ps.ptr<float>();
    for (int r = 0; r < ps.rows / 4; r++)
    {
        cv::Affine3f ap(pfsss + r * 16);
        paths.push_back(ap.inv());
    }
    readFromFile();
    //
    for (int i = 0; i < ps.rows / 4;)
    {
        // cout << "mask2depth" << pqmask->Size() << " " << pqshica->Size() << " " << to3dr->Size() << endl;

        // cv::Mat Frame1out;
        // cv::Mat Frame2;
        // pqshica->Pop(Frame2);
        pqmask->Pop(pf);
        // int *pindx = pf->idx.ptr<int>();

        // float *pfsss = pf->pose.ptr<float>();
        // for (int r = 0; r < pf->idx.rows; r++)
        // {
        //     cout << r << endl;

        //     auto val = pf->idx.at<cv::Vec3i>(r, 0);
        //     cout << "**************" << pf->idx.rows << " " << val[0] << " " << as[val[0] * 2 + 1] << endl;

        f.color = cv::imread(as[i * 2 + 1]);
        f.depth = cv::imread(as[i * 2], 2);
        cv::imshow("a", f.color);
        f.depth2cloud();

        //     cv::Mat c(4, 4, CV_32FC1, pfsss + r * 16);
        //     //     // cv::Mat c(4, 4, CV_32FC1, pfsss + (r+1) * 16);
        //     cv::Affine3f ap(c);
        //     cout << ap.matrix << endl;
        viz.showWidget("coor" + to_string(i), cv::viz::WCoordinateSystem(1.0), paths[i]);
        //     paths.push_back(ap);
        if (paths.size() > 0)
        {
            //         // paths= paths.reshape(0, 16); //修改行数和通道数
            //         // std::cout << "通道: " << paths.channels() << std::endl;
            viz.showWidget("path", cv::viz::WTrajectory(paths));
        }
        viz.showWidget("warp_field" + to_string(i), cv::viz::WCloud(f.cv_cloud, f.cv_color), paths[i]);
        while (1)
        {
            viz.spinOnce(3, false);
            char n = cv::waitKey(10);
            if (n == 'n')
                break;
        }
        i += 100;
        // }
        // break;
    }
}
void funcm4(ImageSyncQueue<Frame *> *pqmask)
{
    viz.showWidget("cooss1r", cv::viz::WCoordinateSystem(1.0));

    std::fstream fss("/home/u20/ros/src/markermapper/img/2023_05_09_16_05_02/file.txt", std::ios::in);
    // cv::FileStorage fs("/home/u20/ros/src/markermapper/img/2023_05_09_16_05_02/pose.yml", cv::FileStorage::READ);

    // cv::Mat idx;
    // fs["idx"] >> idx;
    // std::cout << idx << std::endl;

    std::vector<string> as;
    for (int i = 0; i < 2000; ++i)
    {
        string s1;
        fss >> s1;
        as.push_back("/home/u20/ros/src/markermapper/img/2023_05_09_16_05_02" + s1);
    }
    fss.close();
    frame f;

    Frame *pf;
    int i = 0;

    while (1)
    {

        pqmask->Pop(pf);
        // cout << pf->pose << endl;

        cv::FileStorage fs("mpose5.yml", cv::FileStorage::WRITE);
        fs << "pose" << pf->pose;
        fs << "idx" << pf->idx;
        fs.release();
        system("python3 a.py");
        continue;
        cout << pf->idx << endl;

        int *pindx = pf->idx.ptr<int>();
        float *pfsss = pf->pose.ptr<float>();
        viz.removeAllWidgets();
        for (int r = 0; r < pf->idx.rows; r++)
        {
            i = pindx[r * 3];
            cout << "i=" << i << endl;
            f.color = cv::imread(as[i * 2 + 1]);
            f.depth = cv::imread(as[i * 2], 2);
            f.depth2cloud();
            cv::Affine3f ap(pfsss + r * 16);
            cout << ap.matrix << endl;
            viz.showWidget("warp_field" + to_string(i), cv::viz::WCloud(f.cv_cloud, f.cv_color), ap.inv());
        }
        viz.spinOnce(3, false);

        delete pf;
    }
}
int main()
{
    ImageSyncQueue<Frame *> *pose = new ImageSyncQueue<Frame *>(10);

    // thread thd_3dr(funcm4, pose);
    // thd_3dr.join();
    main222(pose);
}